/******************************************************************************
 * Copyright 2022 The Firmament Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include <firmament.h>
#include "hal/rc/ppm.h"
#include "hal/rc/rc.h"
#include "hal/rc/sbus.h"
#include <ipc/ringbuffer.h>
#include "device.h"
#include "f28p65x_device.h"

#define USE_PPM
#define USE_SBUS

#ifndef min // mod by prife
    #define min(x, y) (x < y ? x : y)
#endif

/* capture accuracy is 0.001ms */
#define PPM_DECODER_FREQUENCY 1000000

/* default config for rc device */
#define RC_CONFIG_DEFAULT                      \
    {                                          \
        RC_PROTOCOL_AUTO, /* auto */           \
            6,            /* 6 channel */      \
            0.05f,        /* sample time */    \
            1000,         /* minimal 1000us */ \
            2000,         /* maximal 2000us */ \
    }

static ppm_decoder_t  ppm_decoder;
static sbus_decoder_t sbus_decoder;

#ifdef USE_PPM
/* PPM */
typedef struct __ppm_capture_gpio_t {
    uint32_t pin;
    uint32_t pin_mux;
    uint32_t xbar_input;
    uint32_t ecap_input;
    uint32_t ecap_base;
    uint32_t ecap_clock;
    uint32_t ecap_interrupt;
} ppm_capture_t;

#define __ECAP_PIN_MUX_INIT(num, xbar, base)        \
    {                                               \
        .pin        = num,                          \
        .pin_mux    = GPIO_##num##_GPIO##num,       \
        .xbar_input = XBAR_INPUT##xbar,             \
        .ecap_input = ECAP_INPUT_INPUTXBAR##xbar,   \
        .ecap_base  = base##_BASE,                  \
        .ecap_clock = SYSCTL_PERIPH_CLK_##base,     \
        .ecap_interrupt = INT_##base,               \
    }

static ppm_capture_t ppm_ecap[] = {
    /*----------------- pin   xbar  ecaps  -*/
    __ECAP_PIN_MUX_INIT(205,   7,    ECAP1),         // PPM: eCAP1 - GPIO205 - XBAR7
};

static uint8_t is_fisrt_frame = 0;

__interrupt void ecap1_isr(void)
{
    uint32_t ecap_base = ECAP1_BASE;
    uint32_t cap1;
    static uint32_t ppm_width = 0;
    static uint8_t i = 0;

    /* enter interrupt */
    rt_interrupt_enter();

    /* polling eCAP interrupt source */
    if (ECAP_getInterruptSource(ecap_base) & ECAP_ISR_SOURCE_CAPTURE_EVENT_1) {

        /* Clear the interrupt flag */
        ECAP_clearInterrupt(ecap_base, ECAP_ISR_SOURCE_CAPTURE_EVENT_1);

        /* Read the captured values */
        cap1 = ECAP_getEventTimeStamp(ecap_base, ECAP_EVENT_1) / 2; // 系统时钟由100MHz改为200MHz, 计数值除2

        /**
         * 系统时钟 100MHz, ecap计数器频率需要缩放到 1MHz = 1us. 因此 ECAP 计数器的值需要除以 100.
         * 帧间隔时间 超过通道的范围(0~2000): 取值 (3000 * 100) 大一些 判断保证准确性.
         */
        if ( cap1 > (3000 * 100) ) {
            ppm_width = 0;
            ppm_update(&ppm_decoder, ppm_width);

            is_fisrt_frame = 1;
            i = 0;
            // rt_kprintf("--->:\r\n");
        } else {
            /* 判断第一次是完整的有效帧, 防止刚上电时, 捕获处在帧中间的干扰 */
            if (1 == is_fisrt_frame) {
                /* calculate ppm pulse width */
                ppm_width += ((cap1 + 1) / 100); // 缩放到 1us, ppm_update 需要的单位是微秒
                ppm_update(&ppm_decoder, ppm_width);

                i++;
                // rt_kprintf("[%d]: %d, %d\r\n", i, ppm_width, cap1/100);
            }
        }

        /* re-arm ecap */
        // ECAP_reArm(ecap_base);

    } else if (ECAP_getInterruptSource(ecap_base) & ECAP_ISR_SOURCE_COUNTER_OVERFLOW) {
        /* Clear the interrupt flag */
        ECAP_clearInterrupt(ecap_base, ECAP_ISR_SOURCE_COUNTER_OVERFLOW);

        /* re-arm ecap */
        // ECAP_reArm(ecap_base);
    }

    //
    // Clear interrupt flags for Global interrupts.
    //
    ECAP_clearGlobalInterrupt(ecap_base);

    //
    // Acknowledge the group interrupt for more interrupts.
    //
    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP4);

    /* leave interrupt */
    rt_interrupt_leave();
}

static rt_err_t ppm_lowlevel_init(void)
{
    ppm_capture_t *pecap = &ppm_ecap[0];

    /* GPIO -> XBAR -> eCAP1 */
    uint32_t ecap_base  = pecap->ecap_base;
    uint32_t ecap_xbar_input = pecap->ecap_input;

    /* gpio config */
    GPIO_setPinConfig(pecap->pin_mux);
    GPIO_setPadConfig(pecap->pin, GPIO_PIN_TYPE_STD);
    GPIO_setQualificationMode(pecap->pin, GPIO_QUAL_ASYNC);
    GPIO_setDirectionMode(pecap->pin, GPIO_DIR_MODE_IN);

    /* TODO: xbar-gpio config */
    XBAR_setInputPin(INPUTXBAR_BASE, pecap->xbar_input, pecap->pin);

    /* ecap config */
    SysCtl_enablePeripheral(pecap->ecap_clock);

    /*  Disable, clear all capture flags and interrupts */
    ECAP_disableInterrupt(ecap_base,
        (ECAP_ISR_SOURCE_CAPTURE_EVENT_1  |
        ECAP_ISR_SOURCE_CAPTURE_EVENT_2  |
        ECAP_ISR_SOURCE_CAPTURE_EVENT_3  |
        ECAP_ISR_SOURCE_CAPTURE_EVENT_4  |
        ECAP_ISR_SOURCE_COUNTER_OVERFLOW |
        ECAP_ISR_SOURCE_COUNTER_PERIOD   |
        ECAP_ISR_SOURCE_COUNTER_COMPARE));

    ECAP_clearInterrupt(ecap_base,
        (ECAP_ISR_SOURCE_CAPTURE_EVENT_1  |
        ECAP_ISR_SOURCE_CAPTURE_EVENT_2  |
        ECAP_ISR_SOURCE_CAPTURE_EVENT_3  |
        ECAP_ISR_SOURCE_CAPTURE_EVENT_4  |
        ECAP_ISR_SOURCE_COUNTER_OVERFLOW |
        ECAP_ISR_SOURCE_COUNTER_PERIOD   |
        ECAP_ISR_SOURCE_COUNTER_COMPARE));

    //
    // Disables time stamp capture.
    //
    ECAP_disableTimeStampCapture(ecap_base);
    //
    // Stops Time stamp counter.
    //
    ECAP_stopCounter(ecap_base);

    //
    // Sets eCAP in Capture mode.
    //
    ECAP_enableCaptureMode(ecap_base);
    //
    // Sets the capture mode: one-shot, stop at 4th event.
    //
    ECAP_setCaptureMode(ecap_base, ECAP_CONTINUOUS_CAPTURE_MODE, ECAP_EVENT_1);

    //
    // Sets the Capture event prescaler.
    //
    ECAP_setEventPrescaler(ecap_base, 0U);

    //
    // Sets the Capture event polarity.
    //
    ECAP_setEventPolarity(ecap_base, ECAP_EVENT_1, ECAP_EVNT_RISING_EDGE);

    //
    // Configure counter reset on events
    //
    ECAP_enableCounterResetOnEvent(ecap_base, ECAP_EVENT_1);

    //
    // Select eCAP input.
    //
    ECAP_selectECAPInput(ecap_base, ecap_xbar_input);

    //
    // Sets a phase shift value count.
    //
    ECAP_setPhaseShiftCount(ecap_base, 0U);
    //
    // Enable counter loading with phase shift value.
    //
    ECAP_enableLoadCounter(ecap_base);

    //
    // Starts Time stamp counter for ecap_base.
    //
    ECAP_startCounter(ecap_base);
    //
    // Enables time stamp capture for ecap_base.
    //
    ECAP_enableTimeStampCapture(ecap_base);

    //
    // Re-arms the eCAP module for ecap_base.
    //
    ECAP_reArm(ecap_base);
    //
    // Enables interrupt source for ecap_base.
    //
    ECAP_enableInterrupt(ecap_base, (ECAP_ISR_SOURCE_CAPTURE_EVENT_1));
    ECAP_enableInterrupt(ecap_base, (ECAP_ISR_SOURCE_COUNTER_OVERFLOW));

    // ISR need to be defined for the registered interrupts
    // Interrupt_register(pecap->ecap_interrupt, &ecap1_isr);
    Interrupt_enable(pecap->ecap_interrupt);

    return RT_EOK;
}
#endif /* end of PPM */

#ifdef USE_SBUS
/* SBUS */
static uint8_t is_frame_head = 0;
static uint8_t cnt = 0;

#define UART_BASE UARTB_BASE

__interrupt void uartb_isr(void)
{
    uint8_t ch;
    uint32_t uart_base = UART_BASE;

    rt_interrupt_enter();

    if (SCI_getInterruptStatus(uart_base) & SCI_INT_RXFF) {

        while (SCI_getRxFIFOStatus(uart_base)) {
            ch = SCI_readCharNonBlocking(uart_base);

            if (0x0F == ch && is_frame_head == 0) {
                sbus_input(&sbus_decoder, &ch, 1);
                is_frame_head = 1;
                cnt++;

            } else if (is_frame_head == 1) {
               sbus_input(&sbus_decoder, &ch, 1);
                cnt++;
            }
        }

        if (cnt == SBUS_FRAME_SIZE) {
            is_frame_head = 0;
            cnt           = 0;
        
            /* if it's reading sbus data, we just parse it later */
            if (!sbus_islock(&sbus_decoder)) {
                sbus_update(&sbus_decoder);
            }
        }
    }

    SCI_clearInterruptStatus(uart_base, SCI_INT_RXFF);

    rt_interrupt_leave();
}

#define UART_RX_MUX(uart, rx) GPIO_##rx##_##uart##_RX
#define UART_TX_MUX(uart, tx) GPIO_##tx##_##uart##_TX

// SBUS IN:  GPIO_45_UARTB_RX
static rt_err_t sbus_lowlevel_init(void)
{
    uint32_t uart_base = UART_BASE;

    // Configure UART RX pin
    GPIO_setPinConfig(UART_RX_MUX(UARTB, 45));
    GPIO_setPadConfig(45, GPIO_PIN_TYPE_STD);
    GPIO_setDirectionMode(45, GPIO_DIR_MODE_IN);
    GPIO_setQualificationMode(45, GPIO_QUAL_ASYNC);

    // Configure UART TX pin
    // GPIO_setPinConfig(UART_TX_MUX(SCIB, 40));
    // GPIO_setPadConfig(40, GPIO_PIN_TYPE_STD);
    // GPIO_setDirectionMode(40, GPIO_DIR_MODE_OUT);
    // GPIO_setQualificationMode(40, GPIO_QUAL_ASYNC);

    /* config usart */
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_UARTB);

    /* initialize isr */
    Interrupt_register(INT_UARTB, uartb_isr);
    Interrupt_enable(INT_UARTB);

    /* 100000bps, even parity, two stop bits */
    uint32_t config = SCI_CONFIG_WLEN_8 | SCI_CONFIG_PAR_EVEN | SCI_CONFIG_STOP_TWO;

    SCI_performSoftwareReset(uart_base);
    SCI_setConfig(uart_base, DEVICE_LSPCLK_FREQ, 100000, config);
    SCI_resetChannels(uart_base);
    SCI_clearInterruptStatus(uart_base, SCI_INT_TXFF | SCI_INT_RXFF);
    SCI_enableFIFO(uart_base);
    SCI_enableModule(uart_base);
    SCI_performSoftwareReset(uart_base);

    /* enable Rx interrupt */
    SCI_setFIFOInterruptLevel(uart_base, SCI_FIFO_TX0, SCI_FIFO_RX1);
    SCI_enableInterrupt(uart_base, SCI_INT_RXFF);

    return RT_EOK;
}
#endif  /* end of SBUS */

static rt_err_t rc_control(rc_dev_t rc, int cmd, void* arg)
{
    switch (cmd) {
    case RC_CMD_CHECK_UPDATE: {
        uint8_t updated = 0;

        if (rc->config.protocol == RC_PROTOCOL_SBUS) {
            updated = sbus_data_ready(&sbus_decoder);
        } else if (rc->config.protocol == RC_PROTOCOL_PPM) {
            updated = ppm_data_ready(&ppm_decoder);
        }

        *(uint8_t*)arg = updated;
    } break;

    default:
        break;
    }

    return RT_EOK;
}

static rt_uint16_t rc_read(rc_dev_t rc, rt_uint16_t chan_mask, rt_uint16_t* chan_val)
{
    uint16_t*   index = chan_val;
    rt_uint16_t rb    = 0;

    if (rc->config.protocol == RC_PROTOCOL_SBUS) {
        if (sbus_data_ready(&sbus_decoder) == 0) {
            /* no data received, just return */
            return 0;
        }

        sbus_lock(&sbus_decoder);

        for (uint8_t i = 0; i < min(rc->config.channel_num, sbus_decoder.rc_count); i++) {
            *(index++) = sbus_decoder.sbus_val[i];
            rb += 2;
        }
        sbus_data_clear(&sbus_decoder);

        sbus_unlock(&sbus_decoder);
    } else if (rc->config.protocol == RC_PROTOCOL_PPM) {
        if (ppm_data_ready(&ppm_decoder) == 0) {
            /* no data received, just return */
            return 0;
        }

        ppm_lock(&ppm_decoder);

        for (uint8_t i = 0; i < min(rc->config.channel_num, ppm_decoder.total_chan); i++) {
            if (chan_mask & (1 << i)) {
                *(index++) = ppm_decoder.ppm_val[i];
                rb += 2;
            }
        }
        ppm_data_clear(&ppm_decoder);

        ppm_unlock(&ppm_decoder);
    }

    return rb;
}

const static struct rc_ops rc_ops = {
    .rc_init = NULL,
    .rc_config  = NULL,
    .rc_control = rc_control,
    .rc_read    = rc_read,
};

static struct rc_device rc_dev = {
    .config = RC_CONFIG_DEFAULT,
    .ops    = &rc_ops,
};

rt_err_t drv_rc_init(void)
{
#ifdef USE_PPM
    /* init ppm driver */
    RT_TRY(ppm_lowlevel_init());
    /* init ppm decoder */
    RT_TRY(ppm_decoder_init(&ppm_decoder, PPM_DECODER_FREQUENCY));
#endif

#ifdef USE_SBUS
    /* init sbus driver */
    RT_TRY(sbus_lowlevel_init());
    RT_TRY(sbus_decoder_init(&sbus_decoder));
#endif

    RT_CHECK(hal_rc_register(&rc_dev, "rc", RT_DEVICE_FLAG_RDWR, NULL));

    return RT_EOK;
}

